"""
Utilities for parsing registration responses and post-processing transforms.
"""

from __future__ import annotations

from typing import Any, Dict, Tuple

import numpy as np


def parse_registration_result(result_json: Dict[str, Any]) -> Dict[str, Any]:
    """Validate and parse the registration response payload."""
    status = result_json.get("status")
    if status != "success":
        raise ValueError(f"配准失败，状态: {status}")

    transform_list = result_json.get("transform")
    if transform_list is None:
        raise ValueError("响应中缺少 transform 字段")

    transform_matrix = np.array(transform_list, dtype=np.float64)
    if transform_matrix.shape != (4, 4):
        raise ValueError(f"变换矩阵形状不正确: {transform_matrix.shape}, 期望 (4, 4)")

    metrics = result_json.get("metrics", {})

    return {
        "status": status,
        "transform_matrix": transform_matrix,
        "metrics": {
            "mean_error": metrics.get("mean", 0.0),
            "median_error": metrics.get("median", 0.0),
            "std_error": metrics.get("std", 0.0),
            "min_error": metrics.get("min", 0.0),
            "max_error": metrics.get("max", 0.0),
            "good_ratio": metrics.get("good_ratio", 0.0),
            "threshold": metrics.get("threshold", 0.0),
        },
    }


def apply_transform_to_point_cloud(points: np.ndarray, transform_matrix: np.ndarray) -> np.ndarray:
    """Apply a homogeneous transform to a point cloud."""
    points = np.array(points)
    if points.shape[1] == 3:
        homogeneous_points = np.hstack([points, np.ones((points.shape[0], 1))])
    else:
        homogeneous_points = points

    transformed_points = (transform_matrix @ homogeneous_points.T).T
    return transformed_points[:, :3]


def extract_rotation_translation(transform_matrix: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    """Extract rotation matrix and translation vector from the transform."""
    rotation_matrix = transform_matrix[:3, :3]
    translation_vector = transform_matrix[:3, 3]
    return rotation_matrix, translation_vector


def validate_transform_matrix(transform_matrix: np.ndarray) -> bool:
    """Basic validation for a homogeneous transform matrix."""
    if transform_matrix.shape != (4, 4):
        return False

    expected_last_row = np.array([0, 0, 0, 1])
    if not np.allclose(transform_matrix[3, :], expected_last_row):
        return False

    rotation_matrix = transform_matrix[:3, :3]
    det = np.linalg.det(rotation_matrix)
    return bool(np.isclose(abs(det), 1.0, atol=1e-6))


